//Include the I2C Arduino library
#include <Wire.h>

//7 bit I2C address of this sensor
#define I2C_ADDRESS       0x74

#define REG_CAP_RED       0x06
#define REG_CAP_GREEN	  0x07
#define REG_CAP_BLUE	  0x08
#define REG_CAP_CLEAR	  0x09

#define REG_INT_RED_LO	  0x0A
#define REG_INT_RED_HI	  0x0B
#define REG_INT_GREEN_LO  0x0C
#define REG_INT_GREEN_HI  0x0D
#define REG_INT_BLUE_LO	  0x0E
#define REG_INT_BLUE_HI	  0x0F
#define REG_INT_CLEAR_LO  0x10
#define REG_INT_CLEAR_HI  0x11

#define REG_DATA_RED_LO	  0x40
#define REG_DATA_RED_HI	  0x41
#define REG_DATA_GREEN_LO 0x42
#define REG_DATA_GREEN_HI 0x43
#define REG_DATA_BLUE_LO  0x44
#define REG_DATA_BLUE_HI  0x45
#define REG_DATA_CLEAR_LO 0x46
#define REG_DATA_CLEAR_HI 0x47

//Configure gain here
int redCap;
int greenCap;
int blueCap;
int clearCap;

int redInt;
int greenInt;
int blueInt;
int clearInt;

int ledPin = 5;

char cmd;
char color;
int value;

//Higher numbers = less sencitive
// 0x00 through 0x0f
int redGain = 0x00;
int greenGain = 0x00;
int blueGain = 0x00;
int clearGain = 0x00;

void setup(void){
  Serial.begin(9600);
  Wire.begin();

  pinMode(ledPin, OUTPUT);
  analogWrite(ledPin, 96);

  set_register(REG_CAP_RED, redGain);
  set_register(REG_CAP_GREEN, greenGain);
  set_register(REG_CAP_BLUE, blueGain);
  set_register(REG_CAP_CLEAR, clearGain);

  establishContact();
}

void loop() {
  if (Serial.available() > 0) {
    delay(10);
    cmd = Serial.read();
    if (cmd == 'M') {
      calibrate();
      measure();
    } else if (cmd == 'D') {
      analogWrite(ledPin, 0);
    } else if (cmd == 'L') {
      analogWrite(ledPin, 255);
    }
    else {
      delay(10);
      color = Serial.read();
      value = 0;
      for (int i=0; i<4; i++) {
        delay(10);
        value *= 10;
        value += (int) (Serial.read() - '0');
      }
      
      changeCap(color, value);
    }
  }
}

void establishContact() {
  while (Serial.available() <= 0) {
    delay(300);
  }
}

void performMeasurement() {
  set_register(0x00,0x01);

  while(read_register(0x00) != 0);
}

void measure() {
  performMeasurement();
  int cc = get_readout(REG_DATA_CLEAR_LO);
  int red = get_readout(REG_DATA_RED_LO);
  int green = get_readout(REG_DATA_GREEN_LO);
  int blue = get_readout(REG_DATA_BLUE_LO);

  int cr = read_register(REG_CAP_RED);
  int cg = read_register(REG_CAP_GREEN);
  int cb = read_register(REG_CAP_BLUE);

  Serial.print(red);
  Serial.print(",");
  Serial.print(green);
  Serial.print(",");
  Serial.print(blue);
  Serial.print(",");
  Serial.print(cc);
  Serial.print(",");
  Serial.print(cr);
  Serial.print(",");
  Serial.print(cg);
  Serial.print(",");
  Serial.println(cb);
}

int get_readout(int readRegister) {
  return read_register(readRegister) + (read_register(readRegister+1)<<8);
}

void set_gain(int gainRegister, int gain) {
  if (gain < 4096) {
    uint8_t hi = gain >> 8;
    uint8_t lo = gain;

    set_register(gainRegister, lo);
    set_register(gainRegister+1, hi);
  }
}

void set_register(unsigned char r, unsigned char v){
  Wire.beginTransmission(I2C_ADDRESS);
  Wire.write(r);
  Wire.write(v);
  Wire.endTransmission();
}

unsigned char read_register(unsigned char r){
  unsigned char v;
  Wire.beginTransmission(I2C_ADDRESS);
  Wire.write(r); // register to read
  Wire.endTransmission();

  Wire.requestFrom(I2C_ADDRESS, 1); // read a byte
  while(!Wire.available()) {
    // waiting
  }
  
  v = Wire.read();
  return v;
}

void calibrate() {
  calibrateGain(REG_INT_CLEAR_LO, REG_DATA_CLEAR_LO);
  calibrateGain(REG_INT_RED_LO, REG_DATA_RED_LO);
  calibrateGain(REG_INT_GREEN_LO, REG_DATA_GREEN_LO);
  calibrateGain(REG_INT_BLUE_LO, REG_DATA_BLUE_LO);
}

void calibrateGain(unsigned char reg_int_lo, unsigned char reg_data_lo) {
  int gainFound = 0;
  int upperBox = 4096;
  int lowerBox = 0;
  int half;

  while (!gainFound) {
    half = ((upperBox-lowerBox)/2) + lowerBox;

    if (half == lowerBox) {
      break; //no further halfing possible
    }
    else {
      set_gain(reg_int_lo, half);
      performMeasurement();
      int halfValue = get_readout(reg_data_lo);

      if (halfValue > 1000) {
        upperBox=half;
      }
      else if (halfValue<1000) {
        lowerBox = half;
      }
      else {
        break; //no further halfing possible
      }
    }
  }

  set_gain(reg_int_lo, half);
}

void changeCap(char color, int value) {
  switch (color) {
    case 'R':
      set_register(REG_CAP_RED, value);
      break;
    case 'G':
      set_register(REG_CAP_GREEN, value);
      break;
    case 'B':
      set_register(REG_CAP_BLUE, value);
      break;
    case 'C':
      set_register(REG_CAP_CLEAR, value);
      break;
  }
}
